#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1

// 使用Lolin Lite默认的I2C引脚
#define SDA_PIN 4
#define SCL_PIN 5

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int throttlePin = 34;
const int switchPin = 2;
bool oledEnabled = false;  // OLED启用状态

// OLED初始化函数
void initOLED() {
  // 初始化I2C总线，使用自定义引脚
  Wire.begin(SDA_PIN, SCL_PIN);
  
  // 尝试两种常见的I2C地址
  bool initialized = false;
  initialized = display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  if (!initialized) {
    initialized = display.begin(SSD1306_SWITCHCAPVCC, 0x3D);
  }
  
  if (!initialized) {
    Serial.println(F("SSD1306 display allocation failed"));
    Serial.println(F("请检查I2C连接和地址"));
    for (;;); // 停止执行
  }
  
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  Serial.println(F("OLED初始化成功"));
}

// 显示油门信息
void displayThrottleInfo(int throttlePercent) {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(2);
  display.print("油门: ");
  display.print(throttlePercent);
  display.println("%");
  display.display();
}

// 显示欢迎界面
void displayWelcome() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.println("ESP32油门测试系统");
  display.setTextSize(2);
  display.setCursor(0, 20);
  display.println("已启动");
  display.display();
}

// I2C扫描函数
void scanI2CBus() {
  Serial.println(F("开始扫描I2C总线..."));
  byte count = 0;
  
  Wire.begin(SDA_PIN, SCL_PIN);
  for (byte i = 8; i < 120; i++) {
    Wire.beginTransmission(i);
    if (Wire.endTransmission() == 0) {
      Serial.print(F("找到设备 @ 地址 0x"));
      Serial.println(i, HEX);
      count++;
    }
    delay(2); // 避免过快扫描
  }
  
  if (count == 0) {
    Serial.println(F("未找到I2C设备"));
  } else {
    Serial.print(F("共找到 "));
    Serial.print(count);
    Serial.println(F(" 个I2C设备"));
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(switchPin, INPUT_PULLUP);
  
  // 扫描I2C总线（仅调试时使用）
  scanI2CBus();
  
  initOLED();
  displayWelcome();
  delay(1000);
  display.clearDisplay();
}

void loop() {
  int throttleValue = analogRead(throttlePin);
  bool switchState = digitalRead(switchPin) == LOW;
  int throttlePercent = map(throttleValue, 0, 4095, 0, 100);

  static bool lastSwitchState = false;
  if (switchState && !lastSwitchState) {
    oledEnabled = !oledEnabled;
    if (oledEnabled) {
      displayThrottleInfo(throttlePercent);
      Serial.println(F("OLED已开启"));
    } else {
      display.clearDisplay();
      display.display();
      Serial.println(F("OLED已关闭"));
    }
  }
  lastSwitchState = switchState;

  if (oledEnabled && switchState) {
    displayThrottleInfo(throttlePercent);
  }

  delay(50);
}